
#include "funcdef.h"


/* Sensor Parameter for BMA180 */
Sensor_Param ITG3205_Param = {
		0x68,			//ID;
		ITG3205_ADDR,      	//sensor_slave_addr;
		UNINITIALISED,		//UNINITIALISED;
		STANDARD_MODE,		//Opmode mode;
		NULL,	//max_reg_addr;
		NULL		//min_reg_addr;
};

/*GYRO_T0 is the compensation coefficient under normal temperature*/
float GYRO_T0[12] = {1.00162251568006,0.00347546483546241,-0.0245136961081888,
					 -0.00387787331400745,0.999281410947346,-0.00752924539941364,
					 -0.0166013446849956,0.0115657745425645,1.00931990798532,
					 0.761324406547947,2.64887285182169,-0.420515616182467};
/*GYRO_T is the temperature compensation coefficient*/
float GYRO_T[12] = {9.18279939960400e-07,2.79984830356541e-05,2.43439818507157e-06,
				    -0.000410776187205144,-0.000687619628189308,4.82408986373633e-05,
					-0.0206763669071244,-0.105075956131877,0.0322253525075171,
					0.0485983212396304,0.0797413338117163,-0.109281248141577};


int ITG3205_init(TSK_Ctrl_b *tsk);

/*--------------ITG3200_sample_tsk()-----------------------------
 * This task is set up to run periodically by pending on ITG3200_sem,
 * which is update by BMA180_sample_PRD PRD module.
 *------------------------------------------------ ------*/
void ITG3200_sample_tsk(void)
{
	int error = 0;
	Uint8 temp = 0;
	short rdata[4];
	double data[4];
	float temp_mat[4];
	float data_after_calb[3];
	TSK_Ctrl_b *self = &(SCB.TCB[TSK_ITG3205]);


	SEM_pend(&ITG3200_sem, SYS_FOREVER);
	// Check if the sensor has been initialized
	while(self->status != INITIALISED){
		error = ITG3205_init(self);
		if(error){
			//DEBUG
			LOG_printf(self->trace, "ITG3205 INIT failed: %d\n", error);
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			//DEBUG
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
		}
	}

	//SEM_pend(&ITG3200_sem, SYS_FOREVER);
	vTskTime(self,RESET_TIME);
	/* Initialize task's STS object to current time */
	TSK_settime(TSK_self());


	/* Sensor initialized, perform sampling operations */
	for(;;){
		SEM_pend(&ITG3200_sem, SYS_FOREVER);
		//vTskTime(self,SET_PREV_TIME);
		STS_set(&sts_ITG3205, SCB.TIME_STAMP);
		temp = 0;
		error = ITG3205_READ_INT_STATUS(self->inhd,self->outhd, &temp);
		if(error){
			//DEBUG
			LOG_printf(self->trace, "ITG3205_READ_INT_STATUS failed: %d\n", error);
			//error = GIO_abort(self->inhd);
			//LOG_printf(self->trace, "GIO abort : %d\n", error);
			//error = GIO_abort(self->outhd);
			//LOG_printf(self->trace, "GIO abort : %d\n", error);
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}

		}
		//LOG_printf(self->trace, "ITG3205_READ_INT_STATUS : %x\n", temp);
		if(temp == RAW_DATA_RDY){
			//LOG_printf(self->trace, "DATA AVAILABLE : %lu\n", SCB.TIME_STAMP);
			//new data available
			error = ITG3205_GET_RDATA(self->inhd, self->outhd, rdata);
			if(error){
				//DEBUG
				LOG_printf(&GyroSampleTsk, "ITG3200_GET_RDATA failed: %d\n", error);
				error = GIO_abort(self->inhd);
				//DEBUG
				LOG_printf(self->trace, "GIO abort : %d\n", error);
				error = GIO_abort(self->outhd);
				//DEBUG
				LOG_printf(self->trace, "GIO abort : %d\n", error);
				error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
				//DEBUG
				if(error){
					LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
				}else{
					LOG_printf(self->trace, "RESET IIC succeed\n");
				}
			}else{
				vNavData.rGyro_x = rdata[0];
				vNavData.rGyro_y = rdata[1];
				vNavData.rGyro_z = rdata[2];
				vNavData.rGyro_T = rdata[3];
				ITG3205_SCALE_RDATA( rdata, data);
				vNavData.Gyro_T = data[3];
				data[3] = 1;
				mat_db_to_ft(4, data, temp_mat);
				MatMul(1, 4, 3, temp_mat, GYRO_T0, data_after_calb);	
				
				vNavData.Gyro_x = data_after_calb[0];
				vNavData.Gyro_y = data_after_calb[1];
				vNavData.Gyro_z = data_after_calb[2];
				
			}
		}
		vTskTime(self,DELTA_TIME);
		STS_delta(&sts_ITG3205,SCB.TIME_STAMP);
		/* Get time difference and
		add it to task's STS object */
		TSK_deltatime(TSK_self());
		TSK_yield();
	}
}

/*--------------ITG3200_init()-----------
 * This function performs deviceID check
 * and read out initial values
 * from ITG3200 accelerometer.
 *-------------------------------------*/

/* ------------------------------*/
/* Initial configuration
 * Sampling Rate: 1kHz
 * +/- 2000 deg/sec
 * LPF fc = 98Hz
 * No interrupt
 ------------------------------*/

int ITG3205_init(TSK_Ctrl_b *tsk){
	int error = 0;
	GIO_Handle inhd = tsk->inhd;
	GIO_Handle outhd = tsk->inhd;

	error = ITG3205_RESET(inhd,outhd);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_RESET failed: %d\n", error);
  		return error;
  	}

	error = ITG3205_CONNECT(inhd,outhd);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_CONNECT failed: %d\n", error);
  		return error;
  	}

  	error = ITG3205_SET_INT_LatchMode( inhd, outhd, 0);
  	if(error){
		//DEBUG
		LOG_printf(tsk->trace, "ITG3205_INT_LATCHCLR failed: %d\n", error);
		return error;
	}

  	error = ITG3205_SET_INT_LatchClrMode( inhd, outhd, 0);
  	if(error){
		//DEBUG
		LOG_printf(tsk->trace, "ITG3205_INT_LATCHCLR failed: %d\n", error);
		return error;
	}

  	//Disable interrupt when device is ready (PLL ready after changing clock source)
	error = ITG3205_SET_INT_ITGRDY( inhd, outhd,  1);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_INT_ITGRDY_EN failed: %d\n", error);
  		return error;
  	}
  	//Enable interrupt when device is ready (PLL ready after changing clock source)
	error = ITG3205_SET_INT_RAWRDY( inhd, outhd,  0);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_INT_ITGRDY_EN failed: %d\n", error);
  		return error;
  	}


  	//changing clock source
	error = ITG3205_CLKSEL(inhd,outhd,ITG3205_XPLL);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_CLKSEL failed: %d\n", error);
  		return error;
  	}

  	//wait until device is ready
 /* 	while(!CLKSEL_FINISH){
		error = ITG3205_READ_INT_STATUS(inhd,outhd, &temp);
		LOG_printf(tsk->trace, "INIT:ITG3205_READ_INT_STATUS : %x\n", temp);
		if(error){
			//DEBUG
			LOG_printf(tsk->trace, " ITG3205_READ_INT_STATUS failed: %d\n", error);
			return error;
		}
		if(temp == ITG_RDY){
			//CLKSEL is finished
			CLKSEL_FINISH = TRUE;
		}
  	}*/


  	//Set LPF
	error = ITG3205_SET_LPF(inhd,outhd,ITG3205_LPF_256HZ);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_SET_LPF failed: %d\n", error);
  		return error;
  	}
  	//Set ADC Sample Rate Divider
	error = ITG3205_SET_SRD(inhd,outhd, 0);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_SET_SRD failed: %d\n", error);
  		return error;
  	}

  	//Disable interrupt when device is ready (PLL ready after changing clock source)
	error = ITG3205_SET_INT_ITGRDY( inhd, outhd,  0);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_INT_ITGRDY_EN failed: %d\n", error);
  		return error;
  	}
  	//Enable interrupt when device is ready (PLL ready after changing clock source)
	error = ITG3205_SET_INT_RAWRDY( inhd, outhd,  1);
  	if(error){
  		//DEBUG
  		LOG_printf(tsk->trace, "ITG3205_INT_ITGRDY_EN failed: %d\n", error);
  		return error;
  	}

  	tsk->status = INITIALISED;
  	//DEBUG
  	LOG_printf(tsk->trace, "ITG3205 Initialized Successfully\n");
	return error;
}




